//==========================================================================//
//                                                                          //
//  ANEMOMETER Library          Version 0.1                     March 2012  //
//                                                                          //
//  Written for the Arduino Duemilanove w/ ATmega328                        //
//                                                                          //
//  ANEMOMETER Arduino Library                                              //
//                                                                          //
//  Created by JFLagarde (jeanfrancoislagarde@hotmail.com)                  //
//                                                                          //
//  This file is part of OpenWS (http://code.google.com/p/openws/)          //
//                                                                          //
//==========================================================================//
//                                                                          //
//  Notes:                                                                  //
//                                                                          //
//                                                                          //
//==========================================================================//
//                                                                          //
//  Copyright 2012 Jean-Francois Lagarde (jeanfrancoislagarde@hotmail.com)  //
//  This program is distributed under the terms of                          //
//  the GNU General Public License                                          //
//                                                                          //
//  This program is free software: you can redistribute it and/or modify    //
//  it under the terms of the GNU General Public License as published by    //
//  the Free Software Foundation, either version 3 of the License, or       //
//  (at your option) any later version.                                     //
//                                                                          //
//  This program is distributed in the hope that it will be useful,         //
//  but WITHOUT ANY WARRANTY; without even the implied warranty of          //
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           //
//  GNU General Public License for more details.                            //
//                                                                          //
//  You should have received a copy of the GNU General Public License       //
//  along with this program.  If not, see <http://www.gnu.org/licenses/>.   //
//                                                                          //
//==========================================================================//

#include <ANEMOMETER.h>

ANEMOMETER theAnemometer = ANEMOMETER();

ANEMOMETER::ANEMOMETER() {
    _lastWindSpeed = 0;
    _lastWindDirection = 0;
	_lastMillis = 0;
	_numRevs = 0;
	
	  _index = 0;
	  //numReadings = 40; // 2min/3sec = 40, 10min/3sec = 200
	  _sumWindSpeed = 0;
	  for (int thisReading = 0; thisReading < numReadings; thisReading++) {
		_bufferWindSpeed[thisReading] = 0;
	  }
}

void ANEMOMETER::init() {
	byte pinAnemometer = 2;	// Digital 2
	byte pinVane = 2;		// Analog 2
	init(pinAnemometer, pinVane);
}
void ANEMOMETER::init(byte pinAnemometer, byte pinVane) {
	_pinAnemometer = pinAnemometer;
	_pinVane = pinVane;
	
	pinMode(_pinAnemometer, INPUT);
	digitalWrite(_pinAnemometer, HIGH);
	attachInterrupt(0, countAnemometer, FALLING);
}
void ANEMOMETER::read() {
	_lastWindSpeed = readWindSpeed();
	_lastWindDirection = readWindDirection();
}
unsigned int ANEMOMETER::getWindSpeed() {
	return (_lastWindSpeed/10);
}
unsigned int ANEMOMETER::getWindDirection() {
	return _lastWindDirection;
}
unsigned int ANEMOMETER::getWindGust() {
	return (_newMaxWindSpeed/10);
}
unsigned int ANEMOMETER::getMeanWindSpeed() {
	return (_newMeanWindSpeed/10);
}
unsigned int ANEMOMETER::getMeanWindDirection() {
	return _newMeanWindDirectionDeg;
}


//PRIVATE methods
unsigned int ANEMOMETER::readWindSpeed(){
// 1mph = 1.609344 km/h
// 1mph = 0.868976242 knot
// 1pps = 2.5mph
// 1pps = 4.02336 km/h
// 1pps = 2.172440605 knots

	long speed = 217244; //x100
	
	unsigned long elapsedMillis = millis() - _lastMillis;
	_lastMillis = millis();
	
	speed *= _numRevs;
	speed /= elapsedMillis;
	
	_numRevs = 0;        // Reset counter
	return speed;
}
unsigned int ANEMOMETER::readWindDirection() {
   int sensorValue = analogRead(_pinVane);
  
   // map it to the range of the analog out:
   return map(sensorValue, 51, 972, 0, 3599);
   //return map(sensorValue, 0, 1023, 0, 3599);
   //return map(sensorValue, 47, 947, 0, 359); //5%-95% for inspeed e-vane
}


//=======================================================
// Interrupt handler for anemometer. Called each time the reed
// switch triggers (one revolution).
//=======================================================
void ANEMOMETER::countAnemometer() {
   _numRevs++;
}










void ANEMOMETER::calcWind() {
  // TODO: Progressive beginning if buffer not filled
  bufferWindSpeed(_lastWindSpeed, _index);
  _newMinWindSpeed = getMinWindSpeed(_lastWindSpeed);
  _newMaxWindSpeed = getMaxWindSpeed(_lastWindSpeed);
  //_newMeanWindSpeed = getMeanWindSpeedSimple();
  _newMeanWindSpeed = getMeanWindSpeedComplex(_lastWindSpeed, _lastWindDirection, _index);
  

  _newMeanWindDirectionDeg = getMeanWindDirectionComplex();
  
  _index++; // advance to the next position in the array:  
  // if we're at the end of the array...
  if (_index >= numReadings) {   
    _index = 0; // ...wrap around to the beginning:
  }
}
void ANEMOMETER::bufferWindSpeed(unsigned int newWindSpeed_x100, int index) {
  // Buffer the WindSpeed
  _sumWindSpeed -= _bufferWindSpeed[index];
  _bufferWindSpeed[index] = newWindSpeed_x100;
  _sumWindSpeed += newWindSpeed_x100;
}
unsigned int ANEMOMETER::getMinWindSpeed(unsigned int newWindSpeed_x100) {
  // Calculate the min of the WindSpeed in the buffer
  unsigned int newMinWindSpeed = newWindSpeed_x100;
  for (int thisReading = 0; thisReading < numReadings; thisReading++) {
    if (_bufferWindSpeed[thisReading] < newMinWindSpeed) {
      newMinWindSpeed = _bufferWindSpeed[thisReading];
    }
  }
  return newMinWindSpeed;
}
unsigned int ANEMOMETER::getMaxWindSpeed(unsigned int newWindSpeed_x100) {
  // Calculate the max of the WindSpeed in the buffer
  unsigned int newMaxWindSpeed = newWindSpeed_x100;
  for (int thisReading = 0; thisReading < numReadings; thisReading++) {
	if (_bufferWindSpeed[thisReading] > newMaxWindSpeed) {
      newMaxWindSpeed = _bufferWindSpeed[thisReading];
    }
  }
  return newMaxWindSpeed;
}
unsigned int ANEMOMETER::getMeanWindSpeedSimple() {
  return _sumWindSpeed / numReadings;
}
unsigned int ANEMOMETER::getMeanWindSpeedComplex(unsigned int newWindSpeed_x100, unsigned int newWindDirectionDeg_x10, int index) {
  float newWindDirectionRad = 0.0;
  int newWindX = 0;
  int newWindY = 0;

  newWindDirectionRad = (newWindDirectionDeg_x10 / 572.96); // rad = deg * (2 * pi / 360) = deg * 1/57.29578
  newWindX = (int)(newWindSpeed_x100 * sin(newWindDirectionRad));
  newWindY = (int)(newWindSpeed_x100 * cos(newWindDirectionRad));

  _sumWindX -= _bufferWindX[index];
  _bufferWindX[index] = newWindX;
  _sumWindX += newWindX;

  _sumWindY -= _bufferWindY[index];
  _bufferWindY[index] = newWindY;
  _sumWindY += newWindY;

  return sqrt(_sumWindX*_sumWindX + _sumWindY*_sumWindY)/numReadings;
}
unsigned int ANEMOMETER::getMeanWindDirectionComplex() {
  float newMeanWindDirectionRad = 0.0;
  int newMeanWindDirectionDeg_x10 = 0;
  
  newMeanWindDirectionRad = atan2(_sumWindY,_sumWindX);
  newMeanWindDirectionDeg_x10 = (int)(-newMeanWindDirectionRad * 572.96); 

  if (_sumWindX > 0  && _sumWindY >  0) newMeanWindDirectionDeg_x10 += 0+900;    // 1. Quadrant
  if (_sumWindX < 0  && _sumWindY >  0) newMeanWindDirectionDeg_x10 += 3600+900; // 2. Quadrant
  if (_sumWindX < 0  && _sumWindY <  0) newMeanWindDirectionDeg_x10 += 0+900;    // 3. Quadrant
  if (_sumWindX > 0  && _sumWindY <  0) newMeanWindDirectionDeg_x10 += 0+900;    // 4. Quadrant
  if (_sumWindX > 0  && _sumWindY == 0) newMeanWindDirectionDeg_x10 = 0;
  if (_sumWindX == 0 && _sumWindY >  0) newMeanWindDirectionDeg_x10 = 900;
  if (_sumWindX < 0  && _sumWindY == 0) newMeanWindDirectionDeg_x10 = 1800;
  if (_sumWindX == 0 && _sumWindY <  0) newMeanWindDirectionDeg_x10 = 2700;
  
  return newMeanWindDirectionDeg_x10;
}













  /*
  float newSinWind = 0.0;
  float newCosWind = 0.0;
  
  newSinWind = sin(newWindDirectionRad);
  newCosWind = cos(newWindDirectionRad);
  newWindX = newWindSpeed * newSinWind;
  newWindY = newWindSpeed * newCosWind;
  
  sumSinWind -= bufferSinWind[index];
  bufferSinWind[index] = newSinWind;
  sumSinWind += newSinWind;

  sumCosWind -= bufferCosWind[index];
  bufferCosWind[index] = newCosWind;
  sumCosWind += newCosWind;
  
  float newWindSpeedSquare = 0.0;
  newWindSpeedSquare = newWindSpeed_x100 * newWindSpeed_x100;
  sumWindSpeedSquare -= bufferWindSpeedSquare[index];
  bufferWindSpeedSquare[index] = newWindSpeedSquare;
  sumWindSpeedSquare += newWindSpeedSquare;
  
  stdWindSpeed = sqrt((sumWindSpeedSquare - sumWindSpeed*sumWindSpeed/numReadings)/numReadings);
  stdWindDirectionDeg = sqrt(sq(sumCosWind/numReadings) + sq(sumSinWind/numReadings));
  stdWindDirectionDeg = sqrt(-2*log(stdWindDirectionDeg))*57.296;
  */


/*
	  //bufferWindDirection(newWindDirection_x10, index);
	void bufferWindDirection(unsigned int newWindDirection_x10, int index) {
    // Buffer the WindDirection
  bufferWindDirectionDeg[index] = newWindDirectionDeg_x10;
}
unsigned int getMinWindDirection(unsigned int newWindSpeed_x100) {
  // Calculate the min of the WindDirection in the buffer
  newMinWindDirectionDeg = newWindDirectionDeg_x10;
  for (int thisReading = 0; thisReading < numReadings; thisReading++) {
    if (bufferWindDirectionDeg[thisReading] < newMinWindDirectionDeg) {
      newMinWindDirectionDeg = bufferWindDirectionDeg[thisReading];
    }
  }
  return newMinWindDirectionDeg;
}
unsigned int getMaxWindDirection(unsigned int newWindSpeed_x100) {
  // Calculate the max of the WindDirection in the buffer
  newMaxWindDirectionDeg = newWindDirectionDeg_x10;
  for (int thisReading = 0; thisReading < numReadings; thisReading++) {
	if (bufferWindDirectionDeg[thisReading] > newMaxWindDirectionDeg) {
      newMaxWindDirectionDeg = bufferWindDirectionDeg[thisReading];
    }
  }
  return newMaxWindDirectionDeg;
}
*/